#include "Formation.h"

namespace formation
{
    /**
     * @brief 获取编队队形的相对位置,坐标系位于编队坐标的原点
     * 
     * @param poses 
     * @param distance 
     * @param type 
     */
    void getFormationRelPose(std::vector<Point>& poses,double distance,FormationType type)
    {
        if (type == TRIANGLE){
            getTriangleFormationRelPose(poses,distance);
        }
        else if (type == RECTANGLE){
            getRectFormationRelPose(poses,distance);
        }
        else if (type == CIRCLE){
            getCircleFormationRelPose(poses,distance);
        }
    }
    /**
     * @brief 三角形编队
     * @note 原点位于第一个顶点,剩下从左到右，从上到下
     * @param poses 
     * @param distance 
     */
    void getTriangleFormationRelPose(std::vector<Point>& poses,double distance)
    {
        poses.clear();
        poses.emplace_back(Point(0.0,0.0,0.0));
        poses.emplace_back(Point(-distance,-distance,0.0));
        poses.emplace_back(Point(distance,-distance,0.0));
        poses.emplace_back(Point(-2*distance,-2*distance,0.0));
        poses.emplace_back(Point(0,-2*distance,0.0));
        poses.emplace_back(Point(2*distance,-2*distance,0.0));
    }
    /**
     * @brief 获取矩形编队的相对位置
     * @note 原点位于第一排中间的车
     * @param poses 
     * @param distance 
     */
    void getRectFormationRelPose(std::vector<Point>& poses,double distance)
    {
        poses.clear();
        poses.emplace_back(Point(-distance,0.0,0.0));
        poses.emplace_back(Point(0.0,0.0,0.0));
        poses.emplace_back(Point(distance,0.0,0.0));
        poses.emplace_back(Point(-distance,-distance,0.0));
        poses.emplace_back(Point(0,-distance,0.0));
        poses.emplace_back(Point(distance,-distance,0.0));
    }

    /**
     * @brief 获取圆形编队
     * @note 原点位于圆心，半径为distance,逆时针顺序
     * @param poses 
     * @param distance 
     */
    void getCircleFormationRelPose(std::vector<Point>& poses,double distance)
    {
        poses.clear();
        poses.emplace_back(Point(0.0,distance,0.0));
        poses.emplace_back(Point(-distance*cos(30/57.3),distance*sin(30.0/57.3),0.0));
        poses.emplace_back(Point(-distance*cos(30/57.3),-distance*sin(30.0/57.3),0.0));
        poses.emplace_back(Point(0.0,-distance,0.0));
        poses.emplace_back(Point(distance*cos(30.0/57.3),-distance*sin(30.0/57.3),0.0));
        poses.emplace_back(Point(distance*cos(30.0/57.3),distance*sin(30.0/57.3),0.0));
    }

    /**
     * @brief 生成编队的圆形轨迹,vx = -R*omega*sin(omega*t),vy = Romega*cos(omega*t)
     * 
     * @param omega 
     * @param R 
     * @param T 
     * @param Velocity 
     */
    void generateCircleTraj(double omega,double R,std::vector<double>& T,std::vector<Point>& Velocity)
    {
        const int N = 6;  // 60 DEG 
        double duration = 2*3.14/(N*omega);  // 每一段的间隔
        T.resize(N);
        Velocity.resize(N);
        double v = omega*R;
        for (int i=0;i<N;i++){
            T[i] = duration;
            Velocity[i] = Point(-v*sin(omega*duration*i),v*cos(omega*duration*i),0.0);
        }
    }
    
    /**
     * @brief 生成编队的矩形轨迹
     * 
     * @param v 
     * @param L 
     * @param T 
     * @param Velocity 
     */
    void generateRectTraj(double v,double L,std::vector<double>& T,std::vector<Point>& Velocity)
    {
        const int N = 4;  
        double duration = L/v;  // 每一段的间隔
        T.resize(N);
        Velocity.resize(N);
        T[0] = duration;
        Velocity[0] = Point(0,v,0.0);
        T[1] = duration;
        Velocity[1] = Point(v,0,0.0);
        T[2] = duration;
        Velocity[2] = Point(0,-v,0.0);
        T[3] = duration;
        Velocity[3] = Point(-v,0,0.0);
    }

    /**
     * @brief 生成随机轨迹
     * @param v 最大速度
     * @param T 
     * @param Velocity 
     */
    void generateRandomTraj(double v,std::vector<double>& T,std::vector<Point>& Velocity)
    {
        const int N = 10;
        const int duration = 5;  // 5s 
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_real_distribution<> dis(-v,v);
        T.resize(N);
        Velocity.resize(N);
        for (int i=0;i<N;i++)
        {
            double vx,vy;
            vx = dis(gen);
            vy = dis(gen);
            T[i] = duration;
            Velocity[i] = Point(vx,vy,0.0);
        }
        
    }

}